#include "RobotMesh.h"
#include <algorithm>
#include <iterator>
#include "../Core/GlPlatform.h"
#include "../View/RenderContext.h"

// Define all sizes for body part objects.
// Can turn these into called variables if need be.
#define HEAD_LENGTH		1.5
#define HEAD_RADIUS		1.15

#define OUTER_EYE		.35
#define INNER_EYE		.1
#define MOUTH_SIZE		.15

#define TORSO_LENGTH	5.0
#define TORSO_RADIUS	1.0
#define TORSO_WIDTH		1.75

#define BICEP_LENGTH	2.0
#define BICEP_RADIUS	0.6

#define ARM_LENGTH		2.5
#define ARM_RADIUS		0.5

#define THIGH_LENGTH	3.25
#define THIGH_RADIUS	0.75

#define SHIN_LENGTH		3.0
#define SHIN_RADIUS		0.65

#define HAND_LENGTH		1.5
#define HAND_RADIUS		0.25
#define HAND_WIDTH		3.0

#define FOOT_LENGTH		2.25
#define FOOT_RADIUS		0.25

#define FOOT_WIDTH		3.0

namespace Model {
	namespace {
		std::shared_ptr<GLUquadricObj> makeQuadric() {
			return std::shared_ptr<GLUquadricObj>( gluNewQuadric(), gluDeleteQuadric );
		}
	}

	struct RobotMesh::Impl {
		enum PartIdx {
			rTorso, 
			rHead, 
			eyes, 
			upArmCaps, 
			lowArmCaps, 
			leftShoulder, 
			leftElbow, 
			leftWrist, 
			rightShoulder,
			rightElbow, 
			rightWrist, 
			wristCaps, 
			leftHip, 
			leftKnee, 
			leftAnkle,
			rightHip,
			rightKnee,
			rightAnkle,
			upLegCaps,
			lowLegCaps,
			feetCaps,
			tCaps,
			PartIdxCount
		};

		Impl() {
			float temp[24] = {15.0, 0.0, 0.0, 0.0, 0.0, 0.0, 190.0, -25.0, 25.0, -90.0, 0.0, 190.0, 25.0, -25.0, -90.0, 0.0, 170.0, 0.0, 15.0, 250.0, 200.0, 0.0, 10.0, 260.0};
			std::copy( temp, temp + 24, theta );

			quadrics.reserve( PartIdxCount );
			std::generate_n( std::back_inserter( quadrics ), (int)PartIdxCount, makeQuadric );
			std::for_each( quadrics.begin(), quadrics.end(), [&] ( const std::shared_ptr<GLUquadricObj>& ptr ) {
				gluQuadricDrawStyle(ptr.get(), GLU_FILL);
				gluQuadricOrientation(ptr.get(), GLU_OUTSIDE);
				gluQuadricNormals(ptr.get(), GLU_SMOOTH);
			} );
		}

		void setMode( int mode ) {
			std::for_each( quadrics.begin(), quadrics.end(), [&] ( const std::shared_ptr<GLUquadricObj>& ptr ) {
				gluQuadricDrawStyle(ptr.get(), mode);
			} );
		}

		GLUquadricObj* q( PartIdx idx ) { return quadrics[(int)idx].get(); } 

		float theta[24];
		std::vector<std::shared_ptr<GLUquadricObj>> quadrics;

		void torso()
		{
			glPushMatrix();
			glRotatef(-90.0, 1.0, 0.0, 0.0);
			glScalef(TORSO_WIDTH, 1.0, 1.0);
			gluDisk( q(tCaps), 0.0, TORSO_RADIUS, 10, 10);
			gluCylinder( q(rTorso), TORSO_RADIUS, TORSO_RADIUS, TORSO_LENGTH, 10, 10);
			glTranslatef(0.0, 0.0, TORSO_LENGTH);
			gluDisk( q(tCaps), 0.0, TORSO_RADIUS, 10, 10);
			glPopMatrix();
		}

		void head()
		{
			glPushMatrix();	 
			glRotatef(-90.0, 0.0, 1.0, 0.0);
			glTranslatef(0.0, 0.35*HEAD_LENGTH,0.0);
			glScalef(HEAD_RADIUS, HEAD_LENGTH, HEAD_RADIUS);
			gluSphere( q(rHead), 1.0, 15, 15);

			glScalef(1/HEAD_RADIUS, 1/HEAD_LENGTH, 1/HEAD_RADIUS);
			glTranslatef(HEAD_RADIUS, .25*HEAD_LENGTH, OUTER_EYE+INNER_EYE);
			glRotatef(90.0, 0.0, 1.0, 0.0);
			glRotatef(-16.0, 1.0, 0.0, 0.0);

			gluDisk( q(eyes), INNER_EYE, OUTER_EYE, 10, 10 );  //eye
			glColor3f(1.0, 0.0, 0.0);
			gluSphere( q(eyes), INNER_EYE, 10, 10);	//eyeball
			glColor3f(0.858824, 0.858824, 0.439216);
			glRotatef(180.0, 0.0, 1.0, 0.0);
			gluCylinder( q(eyes), OUTER_EYE, OUTER_EYE, .5*HEAD_RADIUS, 10, 10);	 //socket
			glRotatef(180.0, 0.0, 1.0, 0.0);
			glTranslatef(2*OUTER_EYE,0.0,0.0);
			gluDisk( q(eyes), INNER_EYE, OUTER_EYE, 10, 10 );   //eye
			glColor3f(1.0, 0.0, 0.0);
			gluSphere( q(eyes), INNER_EYE, 10, 10);	//eyeball
			glColor3f(0.858824, 0.858824, 0.439216);
			glRotatef(180.0, 0.0, 1.0, 0.0);
			gluCylinder( q(eyes), OUTER_EYE, OUTER_EYE, .5*HEAD_RADIUS, 10, 10);	 //socket
			glPopMatrix();
		}

		void leftBicep()
		{
			glPushMatrix();
			glRotatef(-90.0, 1.0, 0.0, 0.0);
			gluCylinder( q(leftShoulder), BICEP_RADIUS, BICEP_RADIUS, BICEP_LENGTH, 10, 10);
			glRotatef(180.0, 1.0, 0.0, 0.0);
			gluDisk( q(upArmCaps), 0.0, BICEP_RADIUS, 10, 10);
			glTranslatef(0.0, 0.0, -BICEP_LENGTH);
			gluDisk( q(upArmCaps), 0.0, BICEP_RADIUS, 10, 10);
			glPopMatrix();
		}

		void leftForearm()
		{
			glPushMatrix();
			glRotatef(-90.0, 1.0, 0.0, 0.0);
			gluCylinder( q(leftElbow), ARM_RADIUS, ARM_RADIUS, ARM_LENGTH, 10, 10);
			gluDisk( q(lowArmCaps), 0.0, ARM_RADIUS, 10, 10);
			glTranslatef(0.0, 0.0, ARM_LENGTH);
			gluDisk( q(lowArmCaps), 0.0, ARM_RADIUS, 10, 10);
			glPopMatrix();
		}

		void leftHand()
		{
			glPushMatrix();
			glRotatef(-90.0, 1.0, 0.0, 0.0);
			glScalef(HAND_WIDTH, 1.0, 1.0);
			gluCylinder( q(leftWrist), HAND_RADIUS, HAND_RADIUS, HAND_LENGTH, 10, 10);
			gluDisk( q(wristCaps), 0.0, HAND_RADIUS, 10, 10);
			glTranslatef(0.0, 0.0, HAND_LENGTH);
			gluDisk( q(wristCaps), 0.0, HAND_RADIUS, 10, 10);
			glPopMatrix();
		}

		void rightBicep()
		{
			glPushMatrix();
			glRotatef(-90.0, 1.0, 0.0, 0.0);
			gluCylinder( q(rightShoulder), BICEP_RADIUS, BICEP_RADIUS, BICEP_LENGTH, 10, 10);
			glRotatef(180.0, 1.0, 0.0, 0.0);
			gluDisk( q(upArmCaps), 0.0, BICEP_RADIUS, 10, 10);
			glTranslatef(0.0, 0.0, -BICEP_LENGTH);
			gluDisk( q(upArmCaps), 0.0, BICEP_RADIUS, 10, 10);
			glPopMatrix();
		}

		void rightForearm()
		{
			glPushMatrix();
			glRotatef(-90.0, 1.0, 0.0, 0.0);
			gluCylinder( q(rightElbow), ARM_RADIUS, ARM_RADIUS, ARM_LENGTH, 10, 10);
			gluDisk( q(lowArmCaps), 0.0, ARM_RADIUS, 10, 10);
			glTranslatef(0.0, 0.0, ARM_LENGTH);
			gluDisk( q(lowArmCaps), 0.0, ARM_RADIUS, 10, 10);
			glPopMatrix();
		}

		void rightHand()
		{
			glPushMatrix();
			glRotatef(-90.0, 1.0, 0.0, 0.0);
			glScalef(HAND_WIDTH, 1.0, 1.0);
			gluCylinder( q(rightWrist), HAND_RADIUS, HAND_RADIUS, HAND_LENGTH, 10, 10);
			gluDisk( q(wristCaps), 0.0, HAND_RADIUS, 10, 10);
			glTranslatef(0.0, 0.0, HAND_LENGTH);
			gluDisk( q(wristCaps), 0.0, HAND_RADIUS, 10, 10);
			glPopMatrix();
		}

		void leftThigh()
		{
			glPushMatrix();
			glRotatef(-90.0, 1.0, 0.0, 0.0);
			gluCylinder( q(leftHip), THIGH_RADIUS, THIGH_RADIUS, THIGH_LENGTH,10,10);
			gluDisk( q(upLegCaps), 0.0, THIGH_RADIUS, 10, 10);
			glTranslatef(0.0, 0.0, THIGH_LENGTH);
			gluDisk( q(upLegCaps), 0.0, THIGH_RADIUS, 10, 10);
			glPopMatrix();
		}

		void leftShin()
		{
			glPushMatrix();
			glRotatef(-90.0, 1.0, 0.0, 0.0);
			gluCylinder( q(leftKnee), SHIN_RADIUS, SHIN_RADIUS, SHIN_LENGTH,10,10);
			gluDisk( q(lowLegCaps), 0.0, SHIN_RADIUS, 10, 10);
			glTranslatef(0.0, 0.0, SHIN_LENGTH);
			gluDisk( q(lowLegCaps), 0.0, SHIN_RADIUS, 10, 10);
			glPopMatrix();
		}

		void leftFoot()
		{
			glPushMatrix();
			glRotatef(-90.0, 1.0, 0.0, 0.0);
			glScalef(FOOT_WIDTH, 1.0, 1.0);
			gluCylinder( q(leftAnkle), FOOT_RADIUS, FOOT_RADIUS, FOOT_LENGTH, 10, 10);
			gluDisk( q(feetCaps), 0.0, FOOT_RADIUS, 10, 10);
			glTranslatef(0.0, 0.0, FOOT_LENGTH);
			gluDisk( q(feetCaps), 0.0, FOOT_RADIUS, 10, 10);
			glPopMatrix();
		}

		void rightThigh()
		{
			glPushMatrix();
			glRotatef(-90.0, 1.0, 0.0, 0.0);
			gluCylinder( q(rightHip), THIGH_RADIUS, THIGH_RADIUS, THIGH_LENGTH,10,10);
			gluDisk( q(upLegCaps), 0.0, THIGH_RADIUS, 10, 10);
			glTranslatef(0.0, 0.0, THIGH_LENGTH);
			gluDisk( q(upLegCaps), 0.0, THIGH_RADIUS, 10, 10);
			glPopMatrix();
		}

		void rightShin()
		{
			glPushMatrix();
			glRotatef(-90.0, 1.0, 0.0, 0.0);
			gluCylinder( q(rightKnee), SHIN_RADIUS, SHIN_RADIUS, SHIN_LENGTH,10,10);
			gluDisk( q(lowLegCaps), 0.0, SHIN_RADIUS, 10, 10);
			glTranslatef(0.0, 0.0, SHIN_LENGTH);
			gluDisk( q(lowLegCaps), 0.0, SHIN_RADIUS, 10, 10);
			glPopMatrix();
		}

		void rightFoot()
		{
			glPushMatrix();
			glRotatef(-90.0, 1.0, 0.0, 0.0);
			glScalef(FOOT_WIDTH, 1.0, 1.0);
			gluCylinder( q(rightAnkle), FOOT_RADIUS, FOOT_RADIUS, FOOT_LENGTH, 10, 10);
			gluDisk( q(feetCaps), 0.0, FOOT_RADIUS, 10, 10);
			glTranslatef(0.0, 0.0, FOOT_LENGTH);
			gluDisk( q(feetCaps), 0.0, FOOT_RADIUS, 10, 10);
			glPopMatrix();
		}

		void draw() {
			glPushMatrix(); 

			glRotatef(theta[0], 0.0, 1.0, 0.0);
			glRotatef(theta[1], 1.0, 0.0, 0.0);
			glRotatef(-theta[2], 0.0, 0.0, 1.0);
			torso();
			glPushMatrix();

			glTranslatef(0.0, TORSO_LENGTH+0.5*HEAD_LENGTH, 0.0);
			glRotatef(theta[3], 1.0, 0.0, 0.0);
			glRotatef(theta[4], 0.0, 1.0, 0.0);
			glRotatef(-theta[5], 0.0, 0.0, 1.0);
			head();

			glPopMatrix();
			glPushMatrix();
			glTranslatef(-((TORSO_RADIUS+TORSO_WIDTH/8)+BICEP_RADIUS), 0.9*TORSO_LENGTH, 0.0);
			glRotatef(theta[6], 1.0, 0.0, 0.0);
			glRotatef(theta[7], 0.0, 1.0, 0.0);
			glRotatef(theta[8], 0.0, 0.0, 1.0);
			leftBicep();

			glTranslatef(0.0, BICEP_LENGTH, 0.0);
			glRotatef(theta[9], 1.0, 0.0, 0.0);
			leftForearm();

			glTranslatef(0.0, BICEP_LENGTH, 0.0);
			glRotatef(theta[10], 1.0, 0.0, 0.0);
			leftHand();

			glPopMatrix();
			glPushMatrix();
			glTranslatef((TORSO_RADIUS+TORSO_WIDTH/8)+BICEP_RADIUS, 0.9*TORSO_LENGTH, 0.0);
			glRotatef(theta[11], 1.0, 0.0, 0.0);
			glRotatef(theta[12], 0.0, 1.0, 0.0);
			glRotatef(theta[13], 0.0, 0.0, 1.0);
			rightBicep();

			glTranslatef(0.0, BICEP_LENGTH, 0.0);
			glRotatef(theta[14], 1.0, 0.0, 0.0);
			rightForearm();

			glTranslatef(0.0, BICEP_LENGTH, 0.0);
			glRotatef(theta[15], 1.0, 0.0, 0.0);
			rightHand();

			glPopMatrix();
			glPushMatrix();
			glTranslatef(-(TORSO_RADIUS), 0.1*THIGH_LENGTH, 0.0);
			glRotatef(theta[16], 1.0, 0.0, 0.0);
			glRotatef(theta[17], 0.0, 0.0, 1.0);
			leftThigh();

			glTranslatef(0.0, THIGH_LENGTH, 0.0);
			glRotatef(theta[18], 1.0, 0.0, 0.0);
			leftShin();

			glTranslatef(0.0, THIGH_LENGTH, 0.0);
			glRotatef(theta[19], 1.0, 0.0, 0.0);
			leftFoot();

			glPopMatrix();
			glPushMatrix();
			glTranslatef(TORSO_RADIUS, 0.1*THIGH_LENGTH, 0.0);
			glRotatef(theta[20], 1.0, 0.0, 0.0);
			glRotatef(theta[21], 0.0, 0.0, 1.0);
			rightThigh();

			glTranslatef(0.0, THIGH_LENGTH, 0.0);
			glRotatef(theta[22], 1.0, 0.0, 0.0);
			rightShin();

			glTranslatef(0.0, THIGH_LENGTH, 0.0);
			glRotatef(theta[23], 1.0, 0.0, 0.0);
			rightFoot();

			glPopMatrix();
		}
	};

	RobotMesh::RobotMesh( const Core::Transform3_t<float>& t )
		: impl( new Impl ) {
		mTransform = t;
	}

	void RobotMesh::onRender( View::RenderContext& rx ) {
		glPushMatrix(); {
			const float f = .1f;
			glScalef( f, f, f );
			impl->draw();
		} glPopMatrix();
	}
}